#include "Matrix.h"
#include <math.h>

Matrix::Matrix(int columnLength)
{
    _columnLength = columnLength;
    _size = columnLength*columnLength;
	_matrix = MatrixXd::Identity(_columnLength, _columnLength);
}

Matrix::Matrix(int columnLength, const MatrixXd &m)
{
	_columnLength = columnLength;
	_size = columnLength*columnLength;
	_matrix = m;
}

Matrix::Matrix(const Matrix &m)
{
    _columnLength = m.columnLength();
    _size = m.size();
	_matrix = m.getMatrix();
}

Matrix& Matrix::operator=(Matrix &matrix)
{
	_columnLength = matrix.columnLength();
    _size = matrix.size();
	_matrix = matrix.getMatrix();

    return *this;
}

const double& Matrix::getRowMajorElement(const int i) const
{
    int row = i/_columnLength;
    int col = i%_columnLength;
    return _matrix(row,col);
}

Matrix Matrix::operator+(Matrix &matrix) const
{

	MatrixXd m = _matrix + matrix.getMatrix();

	Matrix returnMatrix(_columnLength, m);

    return returnMatrix;
}

Matrix Matrix::operator-(Matrix &matrix) const
{

	MatrixXd m = _matrix - matrix.getMatrix();

	Matrix returnMatrix(_columnLength, m);

    return returnMatrix;
}

Matrix Matrix::operator*(Matrix &matrix) const
{

	MatrixXd m = _matrix * matrix.getMatrix();

	Matrix returnMatrix(_columnLength, m);

    return returnMatrix;
}

double Matrix::calculateMatrixMultElement (int pos, const double* first, const double* second) const
{
    double result = 0;
    int col = pos/_columnLength;
    int row = pos%_columnLength;
    for(int i = 0; i < _columnLength; ++i){
        result += (first[row+i*_columnLength] * second[_columnLength*col+i]);
    }
    return result;

}

Matrix Matrix::operator*( QuantizationMatrix &matrix ) const
{
    Matrix m(_columnLength);

    for(int i=0; i < _size; ++i)
        m.setElement(i, _matrix(i) * (double)matrix[i]);

    return m;
}

Matrix Matrix::operator / ( QuantizationMatrix &matrix ) const
{
    Matrix m(_columnLength);

    for(int i=0; i < _size; ++i)
        m.setElement(i, _matrix(i) / (double)matrix[i]);

    return m;
}

void Matrix::loadIdentity()
{
	_matrix = MatrixXd::Identity(_columnLength, _columnLength);
}

void Matrix::setMatrix(MatrixXd& matrix)
{
    _matrix = matrix;
}

void Matrix::addColumn(uint8_t *column, int columnNumber)
{
	for(int i = 0; i < _matrix.rows(); ++i)
	{
		_matrix.col(columnNumber)(i) = column[i];
	}
}

void Matrix::round()
{
    for(int i=0; i < _size; ++i) {

        _matrix(i) += 0.5;

        _matrix(i) = floor(_matrix(i));
    }
}

void Matrix::normalize()
{
    for(int i=0; i < _size; ++i)
        _matrix(i) -= 128;
}

void Matrix::deNormalize()
{
    for(int i=0; i < _size; ++i)
    {
        if(_matrix(i) < 0)
            _matrix(i)+=129;
        else
            _matrix(i)+=128;
    }
}

void Matrix::flattenHighValues()
{
    for(int i=0; i < _size; ++i)
    {
        if(_matrix(i) > 127)
            _matrix(i) = 127;
        else if(_matrix(i) < -128)
            _matrix(i) = -128;
    }
}

void Matrix::setMatrixRowMajor(double *matrix)
{
    for(int i=0; i < _columnLength; ++i)
        for(int j=0; j < _columnLength; ++j)
            _matrix(i*_columnLength+j) = matrix[j*_columnLength+i];
}

void Matrix::setMatrixRowMajor(uint8_t *matrix)
{
    for(int i=0; i < _columnLength; ++i)
        for(int j=0; j < _columnLength; ++j)
            _matrix(i*_columnLength+j) = (double) matrix[j*_columnLength+i];

}

int8_t *Matrix::zigzag()
{
    int8_t *list = new int8_t[_size];

    bool insertEnd = false;

    int pos = 0;

    for(int i = 0; i < _columnLength; ++i)
    {
        int k = 0;
        int j = i;
        while(j >= 0)
        {
            int currentPos = j+k*_columnLength;
            if(!insertEnd)
                list[pos] = _matrix(currentPos);
            else
                list[pos+(j-k)] = _matrix(currentPos);
            --j;
            ++k;
            ++pos;
        }
        insertEnd = !insertEnd;
    }

    for(int i = 1; i < _columnLength; ++i)
    {
        int k = i;
        int j = _columnLength-1;
        while(k < _columnLength)
        {
            if(!insertEnd)
                list[pos] = _matrix(j+k*_columnLength);
            else
                list[pos+(j-k)] = _matrix(j+k*_columnLength);
            --j;
            ++k;
            ++pos;

        }
        insertEnd = !insertEnd;
    }


    return list;
}

void Matrix::setElement(int i, double e)
{
    _matrix(i) = e;
}

void Matrix::newMove1(int &c, int &r, int &cMoveMem, int &rMoveMem, int &move)
{
    if( (cMoveMem+c) >= 0 && (rMoveMem+r) >= 0 ) {
        cMoveMem += c;
        rMoveMem += r;
        move += (r+c*_columnLength);
    } else if( (cMoveMem+c) < 0 ) {
        rMoveMem += r;
        move += r;
        c = 1;
        r = -1;
    } else if( (rMoveMem+r) < 0 ) {
        rMoveMem += c;
        move += (c*_columnLength);
        c = -1;
        r = 1;
    }
}

void Matrix::newMove2(int &c, int &r, int &cMoveMem, int &rMoveMem, int &move)
{
    if( (cMoveMem+c) < _columnLength && (rMoveMem+r) < _columnLength ) {
        cMoveMem += c;
        rMoveMem += r;
        move += (r+c*_columnLength);
    } else if( (cMoveMem+c) >= _columnLength ) {
        rMoveMem += r;
        move += r;
        c = -1;
        r = 1;
    } else if( (rMoveMem+r) >= _columnLength ) {
        rMoveMem += c;
        move += (c*_columnLength);
        c = 1;
        r = -1;
    }
}

void Matrix::setSize(int columnLength)
{
    _columnLength = columnLength;
    _size = columnLength*columnLength;
	_matrix = MatrixXd::Identity(_columnLength, _columnLength);
}

uint8_t* Matrix::getMatrixData()
{
    uint8_t* data = new uint8_t[_size];

    for(int i = 0; i < _size; ++i)
        data[i] = (uint8_t)getRowMajorElement(i);

    return data;
}
